namespace GYLite
{
    export class Quaternion
    {
        public static DEFAULT = new Quaternion();
        public static TEMPVector30 = new Vector3;
        public static TEMPVector31 = new Vector3;
        public static TEMPVector32 = new Vector3;
        public static TEMPVector33 = new Vector3;

        public static TEMPMatrix0 = new Matrix4x4;
        public static TEMPMatrix1 = new Matrix4x4;

        public static _tempMatrix3x3 = new Matrix3x3;

        public static NAN = new Quaternion(NaN, NaN, NaN, NaN);
        
        public elements:Float32Array;
        constructor(x:number=0, y:number=0, z:number=0, w:number=1) {            
            this.elements = new Float32Array(4);            
            this.elements[0] = x;
            this.elements[1] = y;
            this.elements[2] = z;
            this.elements[3] = w;
        }
        /**
         *根据缩放值缩放四元数
        *@param scale 缩放值
        *@param out 输出四元数
        */
        public scaling(scaling:number, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            e[0] = f[0] * scaling;
            e[1] = f[1] * scaling;
            e[2] = f[2] * scaling;
            e[3] = f[3] * scaling;
        }

        /**
         *归一化四元数
        *@param out 输出四元数
        */
        public normalize(out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            var x:number = f[0], y:number = f[1], z:number = f[2], w:number = f[3];
            var len:number = x * x + y * y + z * z + w * w;
            if (len > 0) {
                len = 1 / Math.sqrt(len);
                e[0] = x * len;
                e[1] = y * len;
                e[2] = z * len;
                e[3] = w * len;
            }
        }

        /**
         *计算四元数的长度
        *@return 长度
        */
        public length():number {
            var f:Float32Array = this.elements;
            var x:number = f[0], y:number = f[1], z:number = f[2], w:number = f[3];
            return Math.sqrt(x * x + y * y + z * z + w * w);
        }

        /**
         *根据绕X轴的角度旋转四元数
        *@param rad 角度
        *@param out 输出四元数
        */
        public rotateX(rad:number, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            rad *= 0.5;
            var ax:number = f[0], ay:number = f[1], az:number = f[2], aw:number = f[3];
            var bx:number = Math.sin(rad), bw:number = Math.cos(rad);
            e[0] = ax * bw + aw * bx;
            e[1] = ay * bw + az * bx;
            e[2] = az * bw - ay * bx;
            e[3] = aw * bw - ax * bx;
        }

        /**
         *根据绕Y轴的制定角度旋转四元数
        *@param rad 角度
        *@param out 输出四元数
        */
        public rotateY(rad, out):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            rad *= 0.5;
            var ax:number = f[0], ay:number = f[1], az:number = f[2], aw:number= f[3], by:number = Math.sin(rad), bw:number = Math.cos(rad);
            e[0] = ax * bw - az * by;
            e[1] = ay * bw + aw * by;
            e[2] = az * bw + ax * by;
            e[3] = aw * bw - ay * by;
        }

        /**
         *根据绕Z轴的制定角度旋转四元数
        *@param rad 角度
        *@param out 输出四元数
        */
        public rotateZ(rad:number, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            rad *= 0.5;
            var ax:number = f[0], ay:number = f[1], az:number = f[2], aw:number = f[3], bz:number = Math.sin(rad), bw:number = Math.cos(rad);
            e[0] = ax * bw + ay * bz;
            e[1] = ay * bw - ax * bz;
            e[2] = az * bw + aw * bz;
            e[3] = aw * bw - az * bz;
        }

        /**
         *分解四元数到欧拉角（顺序为Yaw、Pitch、Roll），参考自http://xboxforums.create.msdn.com/forums/p/4574/23988.aspx#23988,问题绕X轴翻转超过±90度时有，会产生瞬间反转
        *@param quaternion 源四元数
        *@param out 欧拉角值
        */
        public getYawPitchRoll(out:Vector3):void {
            Vector3.transformQuat(Vector3.ForwardRH, this, Quaternion.TEMPVector31);
            Vector3.transformQuat(Vector3.Up, this, Quaternion.TEMPVector32);
            var upe:Float32Array = Quaternion.TEMPVector32.elements;
            Quaternion.angleTo(Vector3.ZERO, Quaternion.TEMPVector31, Quaternion.TEMPVector33);
            var anglee:Float32Array = Quaternion.TEMPVector33.elements;
            if (anglee[0] == Math.PI / 2) {
                anglee[1] = Quaternion.arcTanAngle(upe[2], upe[0]);
                anglee[2] = 0;
            } else if (anglee[0] == -Math.PI / 2) {
                anglee[1] = Quaternion.arcTanAngle(-upe[2], -upe[0]);
                anglee[2] = 0;
            } else {
                Matrix4x4.createRotationY(-anglee[1], Quaternion.TEMPMatrix0);
                Matrix4x4.createRotationX(-anglee[0], Quaternion.TEMPMatrix1);
                Vector3.transformCoordinate(Quaternion.TEMPVector32, Quaternion.TEMPMatrix0, Quaternion.TEMPVector32);
                Vector3.transformCoordinate(Quaternion.TEMPVector32, Quaternion.TEMPMatrix1, Quaternion.TEMPVector32);
                anglee[2] = Quaternion.arcTanAngle(upe[1], -upe[0]);
            }
            if (anglee[1] <= -Math.PI)
                anglee[1] = Math.PI;
            if (anglee[2] <= -Math.PI)
                anglee[2] = Math.PI;
            if (anglee[1] >= Math.PI && anglee[2] >= Math.PI) {
                anglee[1] = 0;
                anglee[2] = 0;
                anglee[0] = Math.PI - anglee[0];
            }
            ;
            var oe = out.elements;
            oe[0] = anglee[1];
            oe[1] = anglee[0];
            oe[2] = anglee[2];
        }

        /**
         *求四元数的逆
        *@param out 输出四元数
        */
        public invert(out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = this.elements;
            var a0:number = f[0], a1:number = f[1], a2:number = f[2], a3:number = f[3];
            var dot:number = a0 * a0 + a1 * a1 + a2 * a2 + a3 * a3;
            var invDot:number = dot ? 1.0 / dot : 0;
            e[0] = -a0 * invDot;
            e[1] = -a1 * invDot;
            e[2] = -a2 * invDot;
            e[3] = a3 * invDot;
        }

        /**
         *设置四元数为单位算数
        *@param out 输出四元数
        */
        public identity():void {
            var e:Float32Array = this.elements;
            e[0] = 0;
            e[1] = 0;
            e[2] = 0;
            e[3] = 1;
        }

        /**
         *从Array数组拷贝值。
        *@param array 数组。
        *@param offset 数组偏移。
        */
        public fromArray(array:number[], offset:number=0):void {            
            this.elements[0] = array[offset + 0];
            this.elements[1] = array[offset + 1];
            this.elements[2] = array[offset + 2];
            this.elements[3] = array[offset + 3];
        }

        /**
         *克隆。
        *@param destObject 克隆源。
        */
        public cloneTo(destObject:Quaternion):Quaternion {
            var i:number, s:Float32Array, d:Float32Array;
            s = this.elements;
            d = destObject.elements;
            if (s === d) {
                return;
            }
            for (i = 0; i < 4; ++i) {
                d[i] = s[i];
            }
        }

        /**
         *克隆。
        *@return 克隆副本。
        */
        public clone():Quaternion {
            let cls:any;
            cls = this.constructor;
            var dest = new cls();
            this.cloneTo(dest);
            return dest;
        }

        public equals(b:Quaternion):boolean {
            var ae:Float32Array = this.elements;
            var be:Float32Array = b.elements;
            return MathUtils3D.nearEqual(ae[0], be[0]) && MathUtils3D.nearEqual(ae[1], be[1]) && MathUtils3D.nearEqual(ae[2], be[2]) && MathUtils3D.nearEqual(ae[3], be[3]);
        }

        /**
         *计算长度的平方。
        *@return 长度的平方。
        */
        public lengthSquared():number {
            var x:number = this.elements[0];
            var y:number = this.elements[1];
            var z:number = this.elements[2];
            var w:number = this.elements[3];
            return (x * x) + (y * y) + (z * z) + (w * w);
        }

        /**
         *获取四元数的x值
        */
        public get x():number
        {
            return this.elements[0];
        }        

        /**
         *获取四元数的y值
        */
        public get y():number
        {
            return this.elements[1];
        }        

        /**
         *获取四元数的z值
        */
        public get z():number
        {
            return this.elements[2];
        }
        
        /**
         *获取四元数的w值
        */
        public get w():number
        {
            return this.elements[3];
        }

        public static createFromYawPitchRoll(yaw:number, pitch:number, roll:number, out:Quaternion):void {
            var halfRoll:number = roll * 0.5;
            var halfPitch:number = pitch * 0.5;
            var halfYaw:number = yaw * 0.5;
            var sinRoll:number = Math.sin(halfRoll);
            var cosRoll:number = Math.cos(halfRoll);
            var sinPitch:number = Math.sin(halfPitch);
            var cosPitch:number = Math.cos(halfPitch);
            var sinYaw:number = Math.sin(halfYaw);
            var cosYaw:number = Math.cos(halfYaw);
            var oe = out.elements;
            oe[0] = (cosYaw * sinPitch * cosRoll) + (sinYaw * cosPitch * sinRoll);
            oe[1] = (sinYaw * cosPitch * cosRoll) - (cosYaw * sinPitch * sinRoll);
            oe[2] = (cosYaw * cosPitch * sinRoll) - (sinYaw * sinPitch * cosRoll);
            oe[3] = (cosYaw * cosPitch * cosRoll) + (sinYaw * sinPitch * sinRoll);
        }

        public static multiply(left:Quaternion, right:Quaternion, out:Quaternion):void {
            var le:Float32Array = left.elements;
            var re:Float32Array = right.elements;
            var oe:Float32Array = out.elements;
            var lx:number = le[0];
            var ly:number = le[1];
            var lz:number = le[2];
            var lw:number = le[3];
            var rx:number = re[0];
            var ry:number = re[1];
            var rz:number = re[2];
            var rw:number = re[3];
            var a:number = (ly * rz - lz * ry);
            var b:number = (lz * rx - lx * rz);
            var c:number = (lx * ry - ly * rx);
            var d:number = (lx * rx + ly * ry + lz * rz);
            oe[0] = (lx * rw + rx * lw) + a;
            oe[1] = (ly * rw + ry * lw) + b;
            oe[2] = (lz * rw + rz * lw) + c;
            oe[3] = lw * rw - d;
        }

        public static arcTanAngle(x:number, y:number):number {
            if (x == 0) {
                if (y == 1)
                    return Math.PI / 2;
                return -Math.PI / 2;
            }
            if (x > 0)
                return Math.atan(y / x);
            if (x < 0) {
                if (y > 0)
                    return Math.atan(y / x) + Math.PI;
                return Math.atan(y / x) - Math.PI;
            }
            return 0;
        }

        public static angleTo(from:Vector3, location:Vector3, angle:Vector3):void {
            Vector3.subtract(location, from, Quaternion.TEMPVector30);
            Vector3.normalize(Quaternion.TEMPVector30, Quaternion.TEMPVector30);
            angle.elements[0] = Math.asin(Quaternion.TEMPVector30.y);
            angle.elements[1] = Quaternion.arcTanAngle(-Quaternion.TEMPVector30.z, -Quaternion.TEMPVector30.x);
        }

        public static createFromAxisAngle(axis:Vector3, rad:number, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = axis.elements;
            rad = rad * 0.5;
            var s:number = Math.sin(rad);
            e[0] = s * f[0];
            e[1] = s * f[1];
            e[2] = s * f[2];
            e[3] = Math.cos(rad);
        }

        public static createFromMatrix3x3(sou:Matrix3x3, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = sou.elements;
            var fTrace:number = f[0] + f[4] + f[8];
            var fRoot:number;
            if (fTrace > 0.0) {
                fRoot = Math.sqrt(fTrace + 1.0);
                e[3] = 0.5 * fRoot;
                fRoot = 0.5 / fRoot;
                e[0] = (f[5] - f[7]) * fRoot;
                e[1] = (f[6] - f[2]) * fRoot;
                e[2] = (f[1] - f[3]) * fRoot;
            } else {
                var i = 0;
                if (f[4] > f[0])
                    i = 1;
                if (f[8] > f[i * 3 + i])
                    i = 2;
                var j = (i + 1) % 3;
                var k = (i + 2) % 3;
                fRoot = Math.sqrt(f[i * 3 + i] - f[j * 3 + j] - f[k * 3 + k] + 1.0);
                e[i] = 0.5 * fRoot;
                fRoot = 0.5 / fRoot;
                e[3] = (f[j * 3 + k] - f[k * 3 + j]) * fRoot;
                e[j] = (f[j * 3 + i] + f[i * 3 + j]) * fRoot;
                e[k] = (f[k * 3 + i] + f[i * 3 + k]) * fRoot;
            }
            return;
        }

        public static createFromMatrix4x4(mat:Matrix4x4, out:Quaternion):void {
            var me:Float32Array = mat.elements;
            var oe:Float32Array = out.elements;
            var sqrt:number;
            var half:number;
            var scale:number = me[0] + me[5] + me[10];
            if (scale > 0.0) {
                sqrt = Math.sqrt(scale + 1.0);
                oe[3] = sqrt * 0.5;
                sqrt = 0.5 / sqrt;
                oe[0] = (me[6] - me[9]) * sqrt;
                oe[1] = (me[8] - me[2]) * sqrt;
                oe[2] = (me[1] - me[4]) * sqrt;
            } else if ((me[0] >= me[5]) && (me[0] >= me[10])) {
                sqrt = Math.sqrt(1.0 + me[0] - me[5] - me[10]);
                half = 0.5 / sqrt;
                oe[0] = 0.5 * sqrt;
                oe[1] = (me[1] + me[4]) * half;
                oe[2] = (me[2] + me[8]) * half;
                oe[3] = (me[6] - me[9]) * half;
            } else if (me[5] > me[10]) {
                sqrt = Math.sqrt(1.0 + me[5] - me[0] - me[10]);
                half = 0.5 / sqrt;
                oe[0] = (me[4] + me[1]) * half;
                oe[1] = 0.5 * sqrt;
                oe[2] = (me[9] + me[6]) * half;
                oe[3] = (me[8] - me[2]) * half;
            } else {
                sqrt = Math.sqrt(1.0 + me[10] - me[0] - me[5]);
                half = 0.5 / sqrt;
                oe[0] = (me[8] + me[2]) * half;
                oe[1] = (me[9] + me[6]) * half;
                oe[2] = 0.5 * sqrt;
                oe[3] = (me[1] - me[4]) * half;
            }
        }

        public static slerp(left:Quaternion, right:Quaternion, t:number, out:Quaternion):Float32Array {
            var a:Float32Array = left.elements;
            var b:Float32Array = right.elements;
            var oe:Float32Array = out.elements;
            var ax:number = a[0], ay:number = a[1], az:number = a[2], aw:number = a[3], bx:number = b[0], by:number = b[1], bz:number = b[2], bw:number = b[3];
            var omega:number, cosom:number, sinom:number, scale0:number, scale1:number;
            cosom = ax * bx + ay * by + az * bz + aw * bw;
            if (cosom < 0.0) {
                cosom = -cosom;
                bx = -bx;
                by = -by;
                bz = -bz;
                bw = -bw;
            }
            if ((1.0 - cosom) > 0.000001) {
                omega = Math.acos(cosom);
                sinom = Math.sin(omega);
                scale0 = Math.sin((1.0 - t) * omega) / sinom;
                scale1 = Math.sin(t * omega) / sinom;
            } else {
                scale0 = 1.0 - t;
                scale1 = t;
            }
            oe[0] = scale0 * ax + scale1 * bx;
            oe[1] = scale0 * ay + scale1 * by;
            oe[2] = scale0 * az + scale1 * bz;
            oe[3] = scale0 * aw + scale1 * bw;
            return oe;
        }

        public static lerp(left:Quaternion, right:Quaternion, t:number, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = left.elements;
            var g:Float32Array = right.elements;
            var ax = f[0], ay = f[1], az = f[2], aw = f[3];
            e[0] = ax + t * (g[0] - ax);
            e[1] = ay + t * (g[1] - ay);
            e[2] = az + t * (g[2] - az);
            e[3] = aw + t * (g[3] - aw);
        }

        public static add(left:Quaternion, right:Quaternion, out:Quaternion):void {
            var e:Float32Array = out.elements;
            var f:Float32Array = left.elements;
            var g:Float32Array = right.elements;
            e[0] = f[0] + g[0];
            e[1] = f[1] + g[1];
            e[2] = f[2] + g[2];
            e[3] = f[3] + g[3];
        }

        public static dot(left:Vector3, right:Vector3):number {
            var f:Float32Array = left.elements;
            var g:Float32Array = right.elements;
            return f[0] * g[0] + f[1] * g[1] + f[2] * g[2] + f[3] * g[3];
        }

        public static rotationLookAt(forward:Vector3, up:Vector3, out:Quaternion):void {
            Quaternion.lookAt(Vector3.ZERO, forward, up, out);
        }

        public static lookAt(eye:Vector3, target:Vector3, up:Vector3, out:Quaternion):void {
            Matrix3x3.lookAt(eye, target, up, Quaternion._tempMatrix3x3);
            Quaternion.rotationMatrix(Quaternion._tempMatrix3x3, out);
        }

        public static invert(value:Quaternion, out:Quaternion):void {
            var vE:Float32Array = value.elements;
            var oE:Float32Array = out.elements;
            var lengthSq:number = value.lengthSquared();
            if (!MathUtils3D.isZero(lengthSq)) {
                lengthSq = 1.0 / lengthSq;
                oE[0] = -vE[0] * lengthSq;
                oE[1] = -vE[1] * lengthSq;
                oE[2] = -vE[2] * lengthSq;
                oE[3] = vE[3] * lengthSq;
            }
        }

        public static rotationMatrix(matrix3x3:Matrix3x3, out:Quaternion):void {
            var me:Float32Array = matrix3x3.elements;
            var m11:number = me[0];
            var m12:number = me[1];
            var m13:number = me[2];
            var m21:number = me[3];
            var m22:number = me[4];
            var m23:number = me[5];
            var m31:number = me[6];
            var m32:number = me[7];
            var m33:number = me[8];
            var oe:Float32Array = out.elements;
            var sqrt = NaN, half = NaN;
            var scale = m11 + m22 + m33;
            if (scale > 0) {
                sqrt = Math.sqrt(scale + 1);
                oe[3] = sqrt * 0.5;
                sqrt = 0.5 / sqrt;
                oe[0] = (m23 - m32) * sqrt;
                oe[1] = (m31 - m13) * sqrt;
                oe[2] = (m12 - m21) * sqrt;
            } else if ((m11 >= m22) && (m11 >= m33)) {
                sqrt = Math.sqrt(1 + m11 - m22 - m33);
                half = 0.5 / sqrt;
                oe[0] = 0.5 * sqrt;
                oe[1] = (m12 + m21) * half;
                oe[2] = (m13 + m31) * half;
                oe[3] = (m23 - m32) * half;
            } else if (m22 > m33) {
                sqrt = Math.sqrt(1 + m22 - m11 - m33);
                half = 0.5 / sqrt;
                oe[0] = (m21 + m12) * half;
                oe[1] = 0.5 * sqrt;
                oe[2] = (m32 + m23) * half;
                oe[3] = (m31 - m13) * half;
            } else {
                sqrt = Math.sqrt(1 + m33 - m11 - m22);
                half = 0.5 / sqrt;
                oe[0] = (m31 + m13) * half;
                oe[1] = (m32 + m23) * half;
                oe[2] = 0.5 * sqrt;
                oe[3] = (m12 - m21) * half;
            }
        }
        
    }
}